/******************************************************************************
 *  Nano-RK, a real-time operating system for sensor networks.
 *  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
 *  All rights reserved.
 *
 *  This is the Open Source Version of Nano-RK included as part of a Dual
 *  Licensing Model. If you are unsure which license to use please refer to:
 *  http://www.nanork.org/nano-RK/wiki/Licensing
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, version 2.0 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *******************************************************************************/


#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <rt_link.h>
#include <hal.h>
#include <bmac.h>
#include <nrk_defs.h>
#include <nrk_error.h>
#include <ssp_config.h>

#define MAC_ADDR	0x01
#define MY_CHANNEL 17

nrk_task_type RX_TASK;
NRK_STK rx_task_stack[NRK_APP_STACKSIZE];
void rx_task (void);


nrk_task_type TX_TASK;
NRK_STK tx_task_stack[NRK_APP_STACKSIZE];
void tx_task (void);

void nrk_create_taskset ();

#define MY_RX_SLOT 0
#define MY_TX_SLOT 2

uint8_t rx_buf[RF_MAX_PAYLOAD_SIZE];
uint8_t tx_buf[RF_MAX_PAYLOAD_SIZE];

//routing table structures
uint8_t is_neighbour;
uint8_t rssi_value[4];
uint8_t rssi;
routingData_pkt routing_data;

//data collection structures
data_pkt sensor_data;

//hi packet
HGHi_pkt hi_pkt;
HGHi_pkt rcvHi_pkt;

//hello gateway packet
HGHi_pkt helloGW_pkt;
NetworkConfig_pkt rcvNC_pkt;

//config response packet
HGResponse_pkt ack_pkt;
HGResponse_pkt hgr_pkt;

//network schedules
#define GATEWAY_TX_SLOT 0
#define MY_TX_SLOT		2
#define NODE1_TX_SLOT	4
#define NODE2_TX_SLOT	6
#define NODE3_TX_SLOT	8

inline void buildRoutingData_pkt();
inline void buildData_pkt();
inline void buildHi_pkt();
inline void buildHG_pkt();
inline void buildConfigRes_pkt();
inline void buildGatewayRes_pkt();

inline void init_ssp_pkt();
pktCache_entry pkt_cache_entry[PKT_CACHE_SIZE];

inline hi_pkt_process( uint8_t slot, uint8_t rssi ); // Process when it receives Hi Packet
inline hgr_pkt_process( uint8_t slot ); //Hello Gateway Response Packet

void put_in_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no);
uint8_t is_in_pkt_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no);

struct {

	uint8_t seq_no; // The packet origin
	uint8_t src_addr; // The packet origin
	uint8_t rcv_from; //Node addr we received from
	char flag; //To see if this entry has been checked to use to replace 'R' for read and 'N' for not read yet
}hmEntry[PKT_CACHE_SIZE];

void put_in_hmEntry(uint8_t seq_no, uint8_t src, uint8_t rcv_from);
uint8_t lookup_hmEntry(uint8_t seq_no, uint8_t src);

int main ()
{
	
  nrk_setup_ports ();
  nrk_setup_uart (UART_BAUDRATE_115K2);
	
  nrk_init ();
	
  nrk_led_clr (0);
  nrk_led_clr (1);
  nrk_led_clr (2);
  nrk_led_clr (3);
	
  nrk_time_set (0, 0);
	
  rtl_task_config ();							//RT_link_config
	
  nrk_create_taskset ();
  nrk_start ();
	
  return 0;
}

void tx_task ()
{
  uint8_t length, i;
	
  printf ("tx_task PID=%d\r\n", nrk_get_pid ());

  // Random values put here for testing
  is_neighbour = 0x23;
  rssi_value[0] = 0x01;
  rssi_value[1] = 0x02;
  rssi_value[2] = 0x03;
  rssi_value[3] = 0x04;

  rtl_init (RTL_MOBILE);
  rtl_set_channel(MY_CHANNEL);
  rtl_set_schedule( RTL_RX, MY_RX_SLOT, 1 );
  rtl_set_schedule( RTL_TX, MY_TX_SLOT, 1 );
  rtl_start();
	
  rtl_rx_pkt_set_buffer(rx_buf, RF_MAX_PAYLOAD_SIZE);
	
	// init RTL
	rtl_init (RTL_MOBILE);
	rtl_set_channel(MY_CHANNEL);
	
	//set tx and rx schedules
	rtl_set_schedule( RTL_TX, MY_TX_SLOT, 1 );
	rtl_set_schedule( RTL_RX, GATEWAY_TX_SLOT, 1 );
	rtl_set_schedule( RTL_RX, NODE1_TX_SLOT, 1 );
	rtl_set_schedule( RTL_RX, NODE2_TX_SLOT, 1 );
	rtl_set_schedule( RTL_RX, NODE3_TX_SLOT, 1 );
	
	//start rtl
	rtl_start();
	
	rtl_rx_pkt_set_buffer(rx_buf, RF_MAX_PAYLOAD_SIZE);
	
	while(!rtl_ready())  nrk_wait_until_next_period(); 
	
	while (1) {
		// Build a TX packet
		buildRoutingData_pkt();
		
    memcpy(&tx_buf[PKT_DATA_START], &routing_data, sizeof(routingData_pkt));
    length = PKT_DATA_START + sizeof(routingData_pkt);
	
    if (rtl_tx_pkt_check(MY_TX_SLOT) != 0){
      printf ("Pending on slot %d\r\n",MY_TX_SLOT);
    }
    else{
      nrk_led_set(GREEN_LED);
      rtl_tx_pkt(tx_buf,length, MY_TX_SLOT);
      printf("Sending packet on slot %d of length %d\r\n",MY_TX_SLOT, length);
      rtl_wait_until_tx_done(MY_TX_SLOT);
      nrk_led_clr(GREEN_LED);
    }
    nrk_wait_until_next_period();
  }
}


void rx_task()
{
  uint8_t *local_rx_buf;
  uint8_t length, slot, next_hop;
  int8_t rssi;
 
  printf ("rx_task PID=%d\r\n", nrk_get_pid ());

  while (!rtl_ready()) nrk_wait_until_next_period();

  while (1) {
    nrk_led_set (BLUE_LED);
    nrk_led_toggle(RED_LED);
    if (rtl_rx_pkt_check()!=0) {
	nrk_kprintf(PSTR("Message received\r\n"));
	local_rx_buf = rtl_rx_pkt_get(&length, &rssi, &slot);
	switch (length - PKT_DATA_START){
	case 3:
		memcpy(&rcvHi_pkt, &local_rx_buf[PKT_DATA_START],sizeof(HGHi_pkt));
		if ((rcvHi_pkt.src_addr== GATEWAY) && (rcvHi_pkt.dest_addr == MAC_ADDR)){ //Receives response from Gateway for its Hello gateway packet
			if (is_in_pkt_cache(rcvHi_pkt.src_addr, rcvHi_pkt.dest_addr, sizeof(HGHi_pkt),rcvHi_pkt.seq_no) < 0 ){
				put_in_cache(rcvHi_pkt.src_addr, rcvHi_pkt.dest_addr, sizeof(HGHi_pkt),rcvHi_pkt.seq_no); 
				hgr_pkt_process( slot ); 
				hi_pkt_process(slot,rssi);
			}
		}	
		else if ((rcvHi_pkt.src_addr== GATEWAY) && (rcvHi_pkt.dest_addr != MAC_ADDR)){ //Receives Hello Gateway of some node to gateway
			if (is_in_pkt_cache(rcvHi_pkt.src_addr, rcvHi_pkt.dest_addr, sizeof(HGHi_pkt),rcvHi_pkt.seq_no) < 0 ){
				put_in_cache(rcvHi_pkt.src_addr, rcvHi_pkt.dest_addr, sizeof(HGHi_pkt),rcvHi_pkt.seq_no); 
				hi_pkt_process(slot,rssi);
				// add an entry into the table 
				put_in_hmEntry(rcvHi_pkt.seq_no, rcvHi_pkt.src_addr, (slot/2));
				// Transmit this packet as it is
    				memcpy(&tx_buf[PKT_DATA_START], &rcvHi_pkt, sizeof(HGHi_pkt));
			}
		}	
		else if (rcvHi_pkt.dest_addr == BROADCAST){ //Hi Pkt
			hi_pkt_process(slot,rssi);
		}
		break;
	case 4:
		memcpy(&rcvNC_pkt, &local_rx_buf[PKT_DATA_START],sizeof(NetworkConfig_pkt));
		if ((rcvNC_pkt.src_addr == GATEWAY) && (rcvNC_pkt.dest_addr == BROADCAST)){ // Its a network configuration packet
			if (is_in_pkt_cache(rcvNC_pkt.src_addr, rcvNC_pkt.dest_addr, sizeof(NetworkConfig_pkt),rcvNC_pkt.seq_no) < 0){
				put_in_cache(rcvNC_pkt.src_addr, rcvNC_pkt.dest_addr, sizeof(NetworkConfig_pkt),rcvNC_pkt.seq_no); 
				hi_pkt_process(slot,rssi);
				//**** Broadcast this packet
				//**** send network config response back to gateway
			}
		
		}
		else if ((rcvNC_pkt.src_addr == GATEWAY) && (rcvNC_pkt.dest_addr != BROADCAST) && (rcvNC_pkt.configuration_info == MAC_ADDR)){ // It is receiving GATEWAY response of some node for which this node is in route -- configuration_info here is next_hop 
			if (is_in_pkt_cache(rcvNC_pkt.src_addr, rcvNC_pkt.dest_addr, sizeof(NetworkConfig_pkt),rcvNC_pkt.seq_no) < 0){
				put_in_cache(rcvNC_pkt.src_addr, rcvNC_pkt.dest_addr, sizeof(NetworkConfig_pkt),rcvNC_pkt.seq_no); 
				hi_pkt_process(slot,rssi);
				next_hop = lookup_hmEntry(rcvNC_pkt.seq_no, rcvNC_pkt.src_addr);
				hgr_pkt.seq_no = rcvNC_pkt.seq_no; 
				hgr_pkt.src_addr = rcvNC_pkt.src_addr; 
				hgr_pkt.dest_addr = rcvNC_pkt.dest_addr; 
				hgr_pkt.next_hop = next_hop; 
    				memcpy(&tx_buf[PKT_DATA_START], &hgr_pkt, sizeof(HGResponse_pkt));
			}

		}
		break;
  	}
	rtl_rx_pkt_release();
 	nrk_led_clr (GREEN_LED);
  }
  else {
	nrk_wait_until_next_period();
  }
  }
}

void nrk_create_taskset ()
{
	/*
	RX_TASK.task = rx_task;
	nrk_task_set_stk( &RX_TASK, rx_task_stack, NRK_APP_STACKSIZE);
	RX_TASK.prio = 2;
	RX_TASK.FirstActivation = TRUE;
	RX_TASK.Type = BASIC_TASK;
	RX_TASK.SchType = PREEMPTIVE;
	RX_TASK.period.secs = 1;
	RX_TASK.period.nano_secs = 0;
	RX_TASK.cpu_reserve.secs = 0;
	RX_TASK.cpu_reserve.nano_secs = 500 * NANOS_PER_MS;
	RX_TASK.offset.secs = 0;
	RX_TASK.offset.nano_secs = 0;
	nrk_activate_task (&RX_TASK);
	 */
	
	 TX_TASK.task = tx_task;
	 nrk_task_set_stk( &TX_TASK, tx_task_stack, NRK_APP_STACKSIZE);
	 TX_TASK.prio = 25;
	 TX_TASK.FirstActivation = TRUE;
	 TX_TASK.Type = BASIC_TASK;
	 TX_TASK.SchType = PREEMPTIVE;
	 TX_TASK.period.secs = 1;								//put appropriate period here
	 TX_TASK.period.nano_secs = 0;
	 TX_TASK.cpu_reserve.secs = 0;
	 TX_TASK.cpu_reserve.nano_secs = 500 * NANOS_PER_MS;
	 TX_TASK.offset.secs = 0;
	 TX_TASK.offset.nano_secs = 0;
	 nrk_activate_task (&TX_TASK);
}

inline void init_ssp_pkt(){
	routing_data.seq_no = 0;
	hi_pkt.seq_no = 0;
	sensor_data.seq_no = 0;
	helloGW_pkt.seq_no = 0;
	ack_pkt.seq_no = 0;
}

inline void buildRoutingData_pkt(){
	
	routing_data.src_addr = MAC_ADDR;
	routing_data.dest_addr = GATEWAY;
	routing_data.is_neighbour = is_neighbour;
	routing_data.rssi_value[0] = rssi_value[0];
	routing_data.rssi_value[1] = rssi_value[1];
	routing_data.rssi_value[2] = rssi_value[2];
	routing_data.rssi_value[3] = rssi_value[3];
}

inline void buildGatewayRes_pkt(){

}

inline hi_pkt_process(uint8_t slot,uint8_t rssi){

  switch(slot){
	case 2:
             is_neighbour = is_neighbour ||0x10;
             rssi_value[0] = rssi;
	     break;
	case 4:
             is_neighbour = is_neighbour ||0x20;
             rssi_value[1] = rssi;
	     break;
	case 6:
             is_neighbour = is_neighbour ||0x40;
             rssi_value[2] = rssi;
	     break;
	case 8:
             is_neighbour = is_neighbour ||0x80;
             rssi_value[3] = rssi;
	     break;
	default:
	     break;		
  }
}

inline hgr_pkt_process(uint8_t slot ) //Hello Gateway Response Packet
{
  switch(slot){
	case 2:
             is_neighbour = is_neighbour || 0x01;
	     break;
	case 4:
             is_neighbour = is_neighbour || 0x02;
	     break;
	case 6:
             is_neighbour = is_neighbour || 0x04;
	     break;
	case 8:
             is_neighbour = is_neighbour || 0x08;
	     break;
	default:
	     break;		
   }
}

uint8_t is_in_pkt_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no){
        uint8_t i;
        for(i = 0;  i<PKT_CACHE_SIZE; i++){
                if(pkt_cache_entry[i].src_addr == src &&  pkt_cache_entry[i].dest_addr == dest && pkt_cache_entry[i].length == len && pkt_cache_entry[i].seq_no == seq_no)
                        return 1;
        }
        return -1;
}

void put_in_cache(uint8_t src, uint8_t dest, uint8_t len, uint8_t seq_no){
        uint8_t i, oldest, age;
        age = oldest = 0;
        for(i = 0;  i<PKT_CACHE_SIZE; i++){
                if(pkt_cache_entry[i].age > age){
                        oldest = i;
                        age = pkt_cache_entry[i].age;
                }
        }
        pkt_cache_entry[oldest].src_addr = src;
        pkt_cache_entry[oldest].dest_addr = dest;
        pkt_cache_entry[oldest].length = len;
        pkt_cache_entry[oldest].seq_no = seq_no;


        for(i = 0;  i<PKT_CACHE_SIZE; i++){
                pkt_cache_entry[i].age++;
        }

        pkt_cache_entry[oldest].age = 0;
}

uint8_t lookup_hmEntry(uint8_t seq_no, uint8_t src){
        uint8_t i;
        for(i = 0;  i<PKT_CACHE_SIZE; i++){
                if(hmEntry[i].src_addr == src &&  hmEntry[i].seq_no == seq_no)
                        return hmEntry[i].rcv_from;
        }
        return 0;
}


void put_in_hmEntry(uint8_t seq_no, uint8_t src, uint8_t rcv_from){
        uint8_t i;
        for(i = 0;  i<PKT_CACHE_SIZE; i++){
                if(hmEntry[i].flag > 'R'){
                   hmEntry[i].flag = 'N';     
                   hmEntry[i].seq_no= seq_no;     
                   hmEntry[i].src_addr = src;     
                   hmEntry[i].rcv_from = rcv_from;     
                }
        }
}
